#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <iostream>

#define CAP_INDEX 0
#define PUBLISH_FREQUENCY 5

int main(int argc, char *argv[]) {
    ros::init(argc, argv, "collect_image_node");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    image_transport::Publisher image_pub = it.advertise("/collect_image/image", 1);;

    cv::VideoCapture capture(CAP_INDEX);
    capture.set(CV_CAP_PROP_FRAME_WIDTH, 1280);
    capture.set(CV_CAP_PROP_FRAME_HEIGHT, 720);
    // capture.set(CV_CAP_PROP_FPS, 10);
    

    if (!capture.isOpened()) {
        std::cout << "Unable to open: " << CAP_INDEX << std::endl;
        return 0;
    }

    cv::Mat frame;

    int freq_control_value = 30 / PUBLISH_FREQUENCY;
    int freq_count = 0;
    while (ros::ok()) {
        capture >> frame;
        if (frame.empty())
        {
            ROS_ERROR("frame.empty()");
            break;
        }
        
        if(freq_count == freq_control_value)
        {
            freq_count = 0;
            cv_bridge::CvImage cv_img;
            cv_img.header.stamp = ros::Time::now();
            cv_img.encoding = "bgr8";
            cv_img.image = frame;
            image_pub.publish(cv_img.toImageMsg());
            cv::imshow("Frame", frame);
            cv::waitKey(1);
            ros::spinOnce();
        }
        else
        {
            freq_count++;
        }
        
    }
    return 0;
}
